// Overseer.cpp

#include "Overseer.h"

using namespace Droplets;

extern std::map<uint16_t, bool> _killSwitches;
extern bool _verbose;
extern sem_t _lock;
extern uint16_t _activeDropletID;
extern std::map<uint16_t, Droplets::DropletStateData *> _dStates;

Droplets::Overseer::Overseer() 
{

	// Set number of robots & floor power to 5v
	numRobots = NUM_ROBOTS;
	powFloor = FLOOR_POWER;

	// Set the kill switch to off
	killSwitch = false;

	// Assign Robot IDs
	robotIDS = (uint16_t *) malloc(sizeof(uint16_t) * numRobots);
	for(int16_t i = 0; i < numRobots; i++) 
	{
		robotIDS[i] = (uint16_t)i + 1;
		if(_verbose) 
		{
			printf("Robot %d assigned id : %u\n", i + 1, robotIDS[i]);
		}
	}

	// Setup all the global variables
	SetupGlobals();
}

Droplets::Overseer::~Overseer()
{
	
}

void Droplets::Overseer::Start() 
{
	// Robot thread ids
	robotThreads = (pthread_t *) malloc(sizeof(pthread_t) * numRobots);

	// Send each robot its ID
	DropletThreadData **tDataObjs = (DropletThreadData **) malloc(
		sizeof(DropletThreadData *) * numRobots);
	DropletThreadData *tDataObj;

	for(int i = 0; i < numRobots; i++)
	{
		// create the robot's thread data container
		tDataObj = (DropletThreadData *) malloc(sizeof(DropletThreadData));
		tDataObj->dropletID = robotIDS[i];

		// Tell the simulator which program to load in each robot
		tDataObj->program = Droplets::PROGRAM_1;

		// Launch the robot thread
		pthread_create(&robotThreads[i], NULL, Droplets::DropletThread, (void *)tDataObj);
		tDataObjs[i] = tDataObj;
	}

	// start the control thread
	pthread_create(&controlThread, NULL, Droplets::Overseer::ControlLoop, (void *)this);

	LoopTillStop();

	// Join threads and wrap up
	int tmp;
	for(int i = numRobots - 1; i >= 0; i--) 
	{
		tmp = pthread_join(robotThreads[i], NULL);
		if(_verbose)
		{
			printf("Thread with robot id %u exited with return value %d\n", 
				tDataObjs[i]->dropletID, tmp);
		}
		std::free(tDataObjs[i]);
	}

	// Clear the Droplet thread data
	Stop();
}

void Droplets::Overseer::Stop()
{
	// join up with the control thread
	pthread_join(controlThread, NULL);

	// clear the kill switch map
	_killSwitches.clear();

	// clear the robot's state data
	std::map<uint16_t, DropletStateData *>::iterator mapIter;
	for(mapIter = _dStates.begin(); mapIter != _dStates.end(); mapIter++)
	{
		free((DropletStateData *)mapIter->second);
	}
	_dStates.clear();

	// clear the robot ids
	std::free(robotIDS);
	
	// std::free the robot thread ids
	std::free(robotThreads);

	// delete the semaphore
	sem_destroy(&_lock);
}

void Droplets::Overseer::LoopTillStop()
{
	int r1, r2;
	char in[5];
	while(1)
	{
		std::fgets(in, sizeof(in), stdin);
		
		if(!std::strcmp(in, "kill"))
		{
			killSwitch = true;
			// set all the kill switches
			for(int i  = 0; i < numRobots; i++)
			{
				_killSwitches[robotIDS[i]] = true;
				if(_verbose)
				{
					printf("Kill Switch triggered for thread with robot id - %u\n", robotIDS[i]);
				}
			}
			// flush the stdin buffer
			r1 = std::scanf("%*[^\n]"); /* read and discard all non-newlines */
			if (r1 != EOF)
				r2 = std::scanf("%*c"); /* then read and discard the newline */
			
			break;
		}
	}
}

void *Droplets::Overseer::ControlLoop(void *args) 
{
	if(_verbose)
	{
		printf("Overseer Control loop started...\n");
	}
	Overseer *const that = (Overseer *const)args;
	while(!that->killSwitch)
	{
		that->MoveRobots();
		that->OrganizeMessages();
	}

	if(_verbose)
	{
		printf("Overseer Control loop ended...\n");
	}
	return NULL;
}

void Droplets::Overseer::MoveRobots()
{
	sem_wait(&_lock);
	std::map<uint16_t, Droplets::DropletStateData *>::iterator rIter;
	for(rIter = _dStates.begin(); rIter != _dStates.end(); rIter++)
	{
		Sleep(35);
		if(rIter->second->moving)
		{
			// Check if the robot has move far enough
			if(abs(rIter->second->x - rIter->second->tx) < MIN_DISTANCE_TOL &&
				abs(rIter->second->y - rIter->second->ty) < MIN_DISTANCE_TOL)
			{
				rIter->second->moving = false;
				rIter->second->x = rIter->second->tx;
				rIter->second->y = rIter->second->ty;
				rIter->second->dx = 0;
				rIter->second->dy = 0;
				rIter->second->powMotor1 = 0;
				rIter->second->powMotor1 = 0;
				rIter->second->powMotor1 = 0;
					
				if(_verbose)
				{
					printf("Robot ID - %u at pos (%f, %f)\n", rIter->second->robotID,
						rIter->second->x, rIter->second->y);
				}
			}
			else	// Move the robot by a distance per time step 
			{
				rIter->second->x += rIter->second->dx;
				rIter->second->y += rIter->second->dy;
				BoundPos(rIter->second);
			}
		}
		else if(rIter->second->rotating)
		{
			// Check to see if tee robot has rotated far enough
			if(abs(rIter->second->a - rIter->second->ta) < MIN_DISTANCE_TOL)
			{
			}

		}
	}
	sem_post(&_lock);
}

void Droplets::Overseer::OrganizeMessages()
{

}

void Droplets::Overseer::BoundPos(Droplets::DropletStateData *sPtr)
{
	if(sPtr->x < X_MIN || sPtr->x > X_MAX)
	{
		if(sPtr->x < X_MIN) sPtr->x = X_MIN;
		else sPtr->x = X_MAX;
		sPtr->tx = sPtr->x;
		sPtr->dx = 0;
	}
	if(sPtr->y < X_MIN || sPtr->y > X_MAX)
	{
		if(sPtr->y < X_MIN) sPtr->y = X_MIN;
		else sPtr->y = X_MAX;
		sPtr->ty = sPtr->y;
		sPtr->dy = 0;
	}
}

void Droplets::Overseer::SetupGlobals()
{
	// Clear the kill switch map
	_killSwitches.clear();

	// clear the robot state container map
	float xSet = X_START;
	float ySet = Y_START;
	_dStates.clear();
	for(int i = 0; i < numRobots; i++) 
	{
		// Create the robot's state data container and map it to its ID
		DropletStateData *sDataObj = (DropletStateData *) malloc(sizeof(DropletStateData));
		sDataObj->robotID = robotIDS[i];
		
		// Set the robot's starting position and orientation
		sDataObj->a = 0;
		sDataObj->x = xSet;
		sDataObj->y = ySet;
		
		if(_verbose)
		{
			printf("Robot %u position set at (%f, %f)\n", robotIDS[i], sDataObj->x, sDataObj->y);
		}

		xSet += X_STEP;
		if(xSet >= X_MAX)
		{
			xSet = X_START;
			ySet += Y_STEP;
		}
		_dStates[robotIDS[i]] = sDataObj;

		// Set up the kill switch for the robot
		_killSwitches[robotIDS[i]] = false;
	}
	
	// Set no active robot threads
	_activeDropletID = 0;
	
	// Set up the semaphore
	sem_init(&_lock, 0, 1);
}